//IR.c
#include "system.h"

IRSTATE STATE;
IRSTATE prevState = 0;
DIRECTION DIR;
extern INTERRUPTHANDLERS interrupts;

void initIrSensors(int passing)
{
	if (passing)
	{
		STATE = PASSING;
	}
	else STATE = IDLE;
	
	DIR = START;
}

IRSTATE getIRState(void)
{
	return STATE;
}

int checkIrSensors(byte* time)
{
	/*if (prevState != STATE)
	{
		sendChar('S');
		sendChar(STATE);
		prevState = STATE;
	}*/

	switch(STATE)
	{
		case IDLE:	//IR Timer system is waiting for an interrupt
			if(interrupts.ir1Interrupt)	//Check if IR1 had been triggered
			{
				STARTTIME;
				STATE = WAITFORIR2;	//Now wait for the other sensor
				DIR = IR1TO2;		//Direction flag: forward
				interrupts.ir1Interrupt = 0;
				return IDLE;
			}
			else if(interrupts.ir2Interrupt) //Check if IR2 had been triggered
			{
				STARTTIME;
				STATE = WAITFORIR1;	//Now wait for the other sensor
				DIR = IR2TO1;		//Direction flag: backward
				interrupts.ir2Interrupt = 0;
				return IDLE;
			}
			break;			
		case WAITFORIR1:	//IR2 has been triggered, wait for ir1
			if(interrupts.ir1Interrupt) //Wait for IR1 sensor
			{
				STOPTIME;
				STATE = LOGTIME;	//Both senosrs have been triggered, log time	
				interrupts.ir1Interrupt = 0;
				return WAITFORIR1;
			}
			break;
		case WAITFORIR2:	//IR1 has been triggered, wait for ir2
			if(interrupts.ir2Interrupt)	//Wait for IR2 sensor
			{
				STOPTIME;
				STATE = LOGTIME;	//Both senosrs have been triggered, log time
				interrupts.ir2Interrupt = 0;
				return WAITFORIR2;
			}
			break;		
		case LOGTIME:		//Log time data into time array
			//Setup time and direction data
			time[0] = ((TMR3 >> 8) & 0xff);
			time[1] = (TMR3 & 0xff);
			time[2] = ((TMR2 >> 8) & 0xff);
			time[3] = (TMR2 & 0xff);
			time[4] = DIR;
			STATE = PASSING;
			DIR = START;	//Reset direction flag
			return LOGTIME;	//Return flag indicating time data is valid
			break;
		case ENDTEST_F:
			if (IR1) IR1 = 0;
			if (IR2) IR2 = 0;
			if (IR1I && IR2I)
			{
				STOPTIME;
				STATE = LOGTIMEEND;
				return ENDTEST_F;
			}
			else if (!IR1I)
			{
				STATE = PASSING;
				return ENDTEST_F;
			}
			break;
		case ENDTEST_B:
			if (IR1) IR1 = 0;
			if (IR2) IR2 = 0;
			if (IR2I && IR1I)
			{
				STOPTIME;
				STATE = LOGTIMEEND;
				return ENDTEST_B;
			}
			else if (!IR2I)
			{
				STATE = PASSING;
				return ENDTEST_B;
			}
			break;
		case LOGTIMEEND:		//Log time data into time array
			//Setup time and direction data
			time[0] = ((TMR3 >> 8) & 0xff);
			time[1] = (TMR3 & 0xff);
			time[2] = ((TMR2 >> 8) & 0xff);
			time[3] = (TMR2 & 0xff);
			time[4] = DIR;
			time[5] = 0;
			STATE = PASSING;
			DIR = START;	//Reset direction flag
			return LOGTIMEEND;	//Return flag indicating time data is valid
			break;
		case PASSING:			//Wait for sensors to be clear
			IR1 = 0;
			IR2 = 0;
			if(IR1I && IR2I)
			{
				STATE = IDLE;
				interrupts.ir1Interrupt = 0;
				interrupts.ir2Interrupt = 0;
				return PASSING;
			}
			else if (IR1I && !IR2I)
			{
				STARTTIME;
				STATE = ENDTEST_F;
				DIR = FORWARD;
				return PASSING;
			}
			else if (IR2I && !IR1I)
			{
				STARTTIME;
				STATE = ENDTEST_B;
				DIR = BACKWARD;
				return PASSING;
			}
			break;
		default:
			return -1; //Some kind of error
	}
	
	return STATE;
}
